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Abstract — The implementation challenges of cooperative local- 
ization by dual foot-mounted inertial sensors and inter-agent 
ranging are discussed and work on the subject is reviewed. 
System architecture and sensor fusion are identified as key 
challenges. A partially decentralized system architecture based 
on step-wise inertial navigation and step-wise dead reckoning is 
presented. This architecture is argued to reduce the computa- 
tional cost and required communication bandwidth by around 
two orders of magnitude while only giving negligible information 
loss in comparison with a naive centralized implementation. This 
makes a joint global state estimation feasible for up to a platoon- 
sized group of agents. Furthermore, robust and low-cost sensor 
fusion for the considered setup, based on state space transfor- 
mation and marginalization, is presented. The transformation 
and marginalization are used to give the necessary flexibility for 
presented sampling based updates for the inter-agent ranging and 
ranging free fusion of the two feet of an individual agent. Finally, 
characteristics of the suggested implementation are demonstrated 
with simulations and a real-time system implementation. 




Fig. 1: Illustration of the considered localization setup. A group of agents 
are cooperatively localizing themselves without any infrastructure. For this 
purpose, each agent is equipped with dual foot-mounted inertial sensors, a 
ranging device, and a communication (com.) and processing (proc.) device. 
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I. Introduction 

High accuracy, robust, and infrastructure-free pedestrian 
localization is a highly desired ability for, among others, 
military, security personnel, and first responders. Localization 
together with communication are key capabilities to achieve 
situational awareness and to support, manage, and automatize 
individual's or agent group actions and interactions. See 1 1 1- 
|[5| for reviews on the subject. The fundamental information 
sources for the localization are proprioception, exteroception, 
and motion models. Without infrastructure, the exteroception 
must be dependent on prior or acquired knowledge about 
the environment |6|. Unfortunately, in general, little or no 
prior knowledge of the environment is available and exploiting 
acquired knowledge without revisiting locations is difficult. 
Therefore, preferably the localization should primarily rely 
on proprioception and motion models. Proprioception can 
take place on the agent level, providing the possibility to 
perform dead reckoning; or on inter- agent level, providing the 
means to perform cooperative localization. Foot-mounted in- 
ertial navigation, with motion models providing zero-velocity 
updates, constitute a robust, high accuracy, and unique pedes- 
trian dead reckoning capability |,7J-[,1QJ. With open-source 
implementations |11|-|13| and several products appearing on 
the market |[T4|-p7|, dead reckoning by foot-mounted inertial 
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sensors is a readily available technology. In turn, the most 
straight-forward and well studied inter-agent measurement, 
and mean of cooperative localization, is ranging ||18|-|J2Tj. 
Also here, there are multiple (radio) ranging implementations 
available in the research literature |[22|-|[25| and as products 
on the market |'26'|-|'28 1 . Finally, suitable infrastructure-free 
communication equipment for inter-agent communication is 
available off-the-shelf, e.g. p9|-p2|, and processing plat- 
forms are available in abundance. Together this suggests that 
the setup with foot-mounted inertial sensors and inter-agent 
ranging as illustrated in Fig. [T] is suitably used as a base 
setup for any infrastructure-free pedestrian localization system. 
However, despite the mature components and the in principle 
straight-forward combination, cooperative localization with 
this sensor setup remains challenging, and only a few similar 
systems can be found in the literature p3|-p6J and no off- 
the-shelf products are available. 

The challenges with the localization setup lie in the system 
architecture and the sensor fusion. The inter-agent ranging and 
lack of anchor nodes mean that some global state estimation 
is required with a potentially prohibitive large computational 
cost. The global state estimation, the distributed measure- 
ments, and the (required) high sampling rates of the inertial 
sensors mean that a potentially substantial communication is 
needed and that the system may be sensitive to incomplete 
or varying connectivity. The feet are poor placements for 
inter-agent ranging devices and preferably inertial sensors are 
used on both feet meaning that the sensors of an individual 
agent will not be collocated. This gives a high system state 
dimensionality and means that relating the sensory data from 



different sensors to each other is difficult and that local 
communication links on each agent are needed. Further, inter- 
agent ranging errors as well as sensor separations, often 
have far from white, stationary, and Gaussian characteristics. 
Together, this makes fusing ranging and dead reckoning in a 
high integrity and recursive Bayesian manner at a reasonable 
computational cost difficult. 

Unfortunately, the mentioned challenges are inherent to 
the system setup. Therefore, they have to be addressed for 
any practical implementation. However, to our knowledge, 
the implementation issues have only been sparsely covered 
in isolation in the literature and no complete satisfactory 
solution has been presented. Therefore, in this article we 
present solutions to key challenges to the system setup and 
a complete localization system implementation. More specif- 
ically, the considered overall problem is tracking, i.e recur- 
sively estimating, the positions of a group of agents with the 
equipment setup of Fig. [T] The available measurements for 
the tracking are inertial measurements from the dual foot- 
mounted inertial sensors and inter-agent range measurements. 
The position tracking is illustrated in Fig. [2] The measurements 
will be treated as localized to the respective sensors and 
the necessary communication will be handled as an integral 
part of the overall problem. However, we will not consider 
specific communication technologies, but only communica- 
tion constraints that naturally arise in the current scenario 
(low bandwidth and varying connectivity). See |37|-|[4T| and 
references therein for treatment of related networking and 
communication technologies. Also, for brevity, the issues of 
initialization and time synchronization will not be considered. 
See | [42| and | [43| for the solutions used in the real-time system 
implementation . 

To arrive at the key challenges and the solutions, initially, 
in Section [llj the implementation challenges are discussed in 
more detail and related work is reviewed. Following this, we 
address the key challenges and present a cooperative local- 
ization system implementation based on dual foot-mounted 
inertial sensors and inter-agent ranging. The implementation 
is based on a partially decentralized system architecture and 
statistical marginalization and sampling based measurement 
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updates. In Section [nlj the architecture is presented and argued 
to reduce the computational cost and required communication 
by around two orders of magnitude and to make the system 
robust to varying connectivity, while only giving negligible 
information loss. Thereafter, in Section [IV| the sampling based 
measurement updates with required state space transformation 
and marginalization are presented and shown to give a robust 
and low computational cost sensor fusion. Subsequently, in 
Section |VJ the characteristic of the suggested implementation 
is illustrated via simulations and a real-time system imple- 
mentation. The cooperative localization is found to give a 
bounded relative position mean-square-error (MSE) and an 
absolute position MSE inversely proportional to the number 
of agents, in the worst case scenario; and a bounded position 




MSE, in the best case scenario. Finally, Section VI concludes 
the article. 



position X 



Fig. 2: Illustration of the localization estimation problem and the desired 
output from the localization system. The problem is to track (i.e. recursively 
estimate) the positions of multiple agents in three dimensions by inter-agent 
range measurements and inertial measurements from the foot-mounted inertial 
sensors. 



II. Implementation challenges 

The lack of anchor nodes, the distributed nature of the 
system, the error characteristics of the different sensors, and 
the non-collocated sensors of individual agents poses a number 
of implementation challenges for the cooperative localization. 
Broadly speaking, these challenges can be divided into those 
related to: designing an overall system architecture to min- 
imize the required communication and computational cost, 
while making it robust to varying connectivity and retaining 
sufficient information about the coupling of non-collocated 
parts of the system; and fusing the information of different 
parts of the system given the constraints imposed by the 
system architecture and a finite computational power, while 
retaining performance and a high system integrity. In the 
following two subsections, these two overall challenges are 
discussed in more detail and related previous work is reviewed. 



A. System architecture and state estimation 

The system architecture has a strong connection to the 
position/state estimation and the required communication. 
The range of potential system architectures and estimation 
solutions goes from the completely decentralized, in which 
each agent only estimates its own states, to the completely 
centralized, in which all states of all agents are estimated 
jointly. 

A completely decentralized architecture is often used 
in combination with some inherently decentralized belief- 
propagation estimation techniques (33], | [44| , | [45| . The ad- 
vantage of this is that it makes the localization scalable and 
robust to varying and incomplete connectivity between the 
agents. Unfortunately, the belief-propagation discards infor- 
mation about the coupling between agents, leading to reduced 
performance |45|-|48|. See |46| for an explicit treatment of 
the subject. Unfortunately, as will be shown in Section |VJ 
in a system with dead reckoning, inter-agent ranging, and no 
anchor nodes, the errors in the position estimates of different 
agents may become almost perfectly correlated. Consequently, 
discarding these couplings/correlations between agents can 



significantly deteriorate the localization performance and in- 
tegrity. 

In contrast, with a centralized architecture and estimation, 
all correlations can be considered, but instead the state di- 
mensionality of all agents will add up. Unfortunately, due to 
the lack of collocation of the sensors of individual agents, the 
state dimensionality of individual agents will be high. Together 
this means computationally expensive filter updates. Further, 
the distributed nature of the system means that information 
needs to be gathered to perform the sensor fusion. Therefore, 
communication links are needed, both locally on each agent 
as well as on a group level. Inter-agent communication links 
are naturally wireless. However, the foot-mounting of the 
inertial sensors makes cabled connections impractical opting 
for battery powering and local wireless links for the sensors 
as well 1 49 1, |50|. Unfortunately, the expensive filter updates, 
the wireless communication links, and the battery powering 
combines poorly with the required high sampling rates of the 
inertial sensors. With increasing number of agents, the compu- 
tational cost and the required communication bandwidth will 
eventually become a problem. Moreover, an agent which loses 
contact with the fusion center cannot, unless state statistics are 
continually provided, easily carry on the estimation of its own 
states by itself. Also, to recover from an outage when the 
contact is restored, a significant amount of data would have 
to be stored, transferred and processed. 

Obviously, neither of the extreme cases, the completely 
decentralized nor the completely centralized architectures, are 
acceptable. The related problems suggest that some degree of 
decentralization of the estimation is required to cope with the 
state dimensionality and communication problems. However, 
some global book keeping is also required to handle the infor- 
mation coupling. Multiple approximative and exact distributed 
implementations of global state estimation have been demon- 
strated, see 1481 , l[5TJ-|[53l and references therein. However, 
these methods suffer from either a high computational cost 
or required guaranteed and high bandwidth communication, 
and are not adapted to the considered sensor setup with high 
update rates, local communication links, and lack of sensor 



collocation. Therefore, in Section [III] we suggest and motivate 
a system architecture with partially decentralized estimation 
based on a division of the foot-mounted inertial navigation 
into a step- wise inertial navigation and dead reckoning. This 
architecture does not completely solve the computational cost 
issue, but makes it manageable for up to a platoon-sized 
group of agents. For larger groups, some cellular structure 
is needed J34J, JS^. However, the architecture is largely 
independent of how the global state estimation is implemented 
and a distributed implementation is conceivable. 

The idea of dividing the filtering is not completely new. 
A similar division is presented in an application specific 
context in f54l and used to fuse data from foot-mounted 
inertial sensors with maps, or to build the maps themselves, 
in p5l-|[57|. However, the described division is heuristically 
motivated and the statistical relation between the different 
parts is not clear. Also, no physical processing decentralization 
is exploited to give reduced communication requirements. 



B. Robust and low computational cost sensor fusion 

The sensor fusion firstly poses the problem of how to 
model the relation between the tracked inertial sensors and the 
range measurements. Secondly, it poses the problem of how to 
condition the state statistic estimates on provided information 
while retaining reasonable computational costs. 

The easiest solution to the non-collocated sensors of individ- 
ual agents is to make the assumption that they are collocated 
(or have a fixed relation) J33J, |[58j-|60|. While simple, this 
method can clearly introduce modeling errors resulting in 
suboptimal performance and questionable integrity. Instead, 
explicitly modeling the relative motion of the feet has been 
suggested in |61|. However, making an accurate and general 
model of the human motion is difficult, to say the least. As an 
alternative, multiple publications suggest explicitly measuring 
the relation between the sensors | [TQ] |, |[62|-|[64]|. The added 
information can improve the localization performance but 
unfortunately introduces the need for additional hardware and 
measurement models. Also, it works best for situations with 
line-of-sight between measurement points, and therefore, it is 
probably only a viable solution for foot-to-foot ranging on 
clear, not too rough, and vegetation/obstacle free ground |65j. 
Instead of modeling or measuring the relation between nav- 
igation points of an individual agent, the constraint that the 
spatial separation between them has an upper limit may be 
used. This side information obviously has an almost perfect 
integrity, and results in |66| indicate that the performance loss 
in comparison to ranging is transitory. For inertial navigation, 
it has been demonstrated that a range constraint can be used 
to fuse the information from two foot-mounted systems, while 
only propagating the mean and the covariance | [67| , |[68|. 
Unfortunately, the suggested methods depend on numerical 
solvers and only apply the constraint on the mean, giving ques- 



tionable statistical properties. Therefore, in Section [IVj based 
on the work in | [66| , we suggest a simpler and numerically 
more attractive solution to using range constraints to perform 
the sensor fusion, based on marginalization and sampling. 

The naive solution to the sensor fusion of the foot-mounted 
inertial navigation and the inter-agent ranging is simply us- 
ing traditional Kalman filter measurement updates for the 
ranging p3| . However, the radio ranging errors are often 
far from Gaussian, often with heavy tails and non- stationary 
and spatially correlated errors |[69j|-||73J. This can cause 
unexpected behavior of many localization algorithms, and 
therefore, statistically more robust methods are desirable | [73| , 
||74|. See p75l and references therein for a general treatment 
of the statistical robustness concept. The heavy tails and 
spatially correlated errors could potentially be solved by a 
flat likelihood function as suggested in |69|, |76|. However, 
while giving a high integrity, this also ignores a substantial 
amount of information and requires multi-hypothesis filtering 
(a particle filter) with unacceptable high computational cost. 
Using a more informative likelihood function is not hard to 
imagine. Unfortunately, only a small set of likelihood func- 
tions can easily be used without resorting to multi-hypothesis 
filtering methods. Some low cost fusion techniques for special 
classes of heavy tailed distributions and H^o criteria have 



been suggested in the literature |[77|-|[8T|. However, ideally we 
would like more flexibility to handle measurement errors and 
non-collocated sensors. Therefore, in Section llv) we propose 



a marginalization and sample based measurement update for 
the inter-agent ranging, providing the necessary flexibility to 
handle an arbitrary likelihood function. A suitable likelihood 
function is proposed, taking lack of collocation, statistical 
robustness, and correlated errors into account, and shown to 
providing a robust and low computational cost sensor fusion. 

III. Decentralized estimation architecture 

To get around the problems of the centralized architecture, 
the state estimation needs somehow to be partially decen- 
tralized. However, as previously argued, some global state 
estimation is necessary. Consequently, the challenge is to do 
the decentralization in a way that does not lead to unacceptable 
loss in information coupling, leading to poor performance and 
integrity, while still solving the issues with computational 
cost, communication bandwidth, and robustness to varying 
connectivity. In the following subsections it is shown how this 
can be achieved by dividing the filtering associated with foot- 
mounted inertial sensors into a step-wise inertial navigation 
and step- wise dead reckoning, as illustrated in Fig. [5] Pseudo- 
code for the related processing is found in Alg. Tl| and the 
complete system architecture is illustrated in Fig. p] 



A. Zero-velocity-update-aided inertial navigation 

To track the position of an agent equipped with foot- 
mounted inertial sensors, the sensors are used to implement 
an inertial navigation system aided by so called zero- velocity 
updates (ZUPTs). The inertial navigation essentially consists 
of the inertial sensors combined with mechanization equations. 
In the simplest form, the mechanization equations are 
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v/c-1 + (q/c-iffeq^-i - ^)dt 

n{uJkdt)cik-i 



(1) 



where /c is a time index, dt is the time difference between 
measurement instances, p^ is the position, v^ is the velocity, 
ffc is the specific force, g = [0,0,^] is the gravity, and LVk 
is the angular rate (all in R^). Further, q/e is the quaternion 
describing the orientation of the system, the triple product 
q/c_if/eq^_^ denotes the rotation of f/e by q/e, and fl{-) is 
the quaternion update matrix. For a detailed treatment of 
inertial navigation see | [82| , 183] . For analytical convenience 
we will interchangeably represent the orientation q/^ with the 
equivalent Euler angles (roll,pitch,yaw) Ok = [^/c,6>/c, V^/c]. 
Note that [•,...] is used to denote a column vector. 

The mechanization equations ([T]) together with measure- 
ments of the specific force f/c and the angular rates ojk, pro- 
vided by the inertial sensors, are used to propagate position p/^, 
velocity v^, and orientation q/e state estimates. Unfortunately, 
due to its integrative nature, small measurement errors in f/e 
and oJk accumulate, giving rapidly growing estimation errors. 
Fortunately, these errors can be modeled and estimated with 



ZUPTs. A first-order error model of ([T]) is given by 
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where S{-)k are the error states, I and are 3 x 3 iden- 
tity and zero matrices, respectively, and [-Jx is the cross- 
product matrix. As argued in f84l, one should be cautious 
about estimating systematic sensor errors in the current setup. 
Indeed, remarkable dead-reckoning performance has been 
demonstrated, exploiting dual foot-mounted sensors without 
any sensor error state estimation |85|. Therefore, in contrast 
to many publications, no additional sensor bias states are used. 
Together with statistical models for the errors in f/^ and 
a;/c, ([2]) is used to propagate statistics of the error states. To 
estimate the error states, stationary time instances are detected 
based on the condition Z{{i^^6j^}wk) < 7z' where Z{-) is 
some zero-velocity test statistic, {i^^6j^}wk i^ the inertial 
measurements over some time window Wk, and 7z is a zero- 
velocity detection threshold. See |86j|, |[87| for further details. 
The implied zero- velocities are used as pseudo-measurements 



which are modeled in terms of the error states as 



yfc = H 
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where H = [0 I 0] is the measurement matrix and n^ is a 
measurement noise, i.e. fk = 6\'k-\-nk. Given the error model 
([2]) and the measurements model (|4]), the measurements ^ can 
be used to estimate the error states with a Kalman type of filter. 
See Q, |[87)-|[89} for further details and variations. See ||90l 
for a general treatment of aided navigation. Since there is no 
reason to propagate errors, as soon as there are any non-zero 
estimates Spk, Sir^, or SOk, they are fed back correcting the 
navigational states. 



and q/e := n{SOk)cik (5) 



and consequently the error state estimates are set to zero, i.e. 
Spk := Osxi, Svk := Osxi, and dOk := Osxi, where := 
indicates assignment. 

Unfortunately, all (error) states are not observable based on 
the ZUPTs. During periods of consecutive ZUPTs, the system 
^ becomes essentially linear and time invariant. Zero-velocity 
for consecutive time instances means no acceleration and 
ideally f/^ = q^gq/c- This gives the system and observability 
matrices 
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Obviously, the position (error) is not observable, while the 
velocity is. Since 
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the roll and pitch are observable while the heading (yaw) of 
the system is not. Ignoring the process noise, this implies 
that the covariances of the observable states decay as one 
over the number of consecutive ZUPTs. Note that there is 
no difference between the covariances of the error states and 
the states themselves. Consequently, during stand-still, after 
a reasonable number of ZUPTs, the state estimate covariance 
becomes 
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where Px,y=coY{x,y), Paj=cov(x) =cov(x,x), {-)^ denotes 
the transpose, and O^xm denotes a zero matrix of size nxm. 

B. Step-wise dead reckoning 

The covariance matrix ([6]) tells us that the errors of pk and 
tljk are uncorrelated with those of v^ and [(j)k,Ok]- Together 
with the Markovian assumption of the state space models and 
the translational and in-plan rotation invariance of ([l])-(|4]), this 
means that future errors of v^ and [(/)/c, 6>/c] are uncorrelated 
with those of the current pk and V^/e. Consequently, future 
ZUPTs cannot be used to deduce information about the 
current position and heading errors. In turn, this means that, 
considering only the ZUPTs, it makes no difference if we reset 
the system and add the new relative position and heading to 
those before the reset. However, for other information sources, 
we must keep track of the global (total) error covariance of 
the position and heading estimates. 

Resetting the system means setting position pk and heading 
iljk and corresponding covariances to zero. Denote the position 
and heading estimates at a reset i by dp£ and dipi. These 
values can be used to drive the step-wise dead reckoning 
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where x^ and Xi are the global position in 3 dimensions and 
heading in the horizontal plan of the inertial navigation system 
relative to the navigation frame. 



R/ = 



is the rotation matrix from the local coordinate frame of the 
last reset to the navigation frame, and w^ is a (by assumption) 
white noise with covariance. 
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The noise w^ in ^ represents the accumulated uncertainty in 
position and heading since the last reset, i.e. the essentially 
non-zero elements in ([6]) transformed to the navigation frame. 
The dead reckoning ^ can trivially be used to estimate x^ 
and Xi and their error covariances from dpi and dipi and 
related covariances. The relation between the step- wise inertial 
navigation and dead reckoning is illustrated in Fig. [3] 




x^+i 



Step-wise dead reckoning Step-wise inertial navigation 
-• -•- • • • 

Fig. 3: Illustration of the step-wise inertial navigation and the step-wise dead 
reckoning. The displacement and heading change over a step given by the 
inertial navigation is used to perform the step- wise dead-reckoning. 



To get [dp£^ di/ji] from the inertial navigation, reset instances 
need to be determined, i.e. the decoupled situation ^ needs 
to be detected. However, detecting it is not enough. If it holds 
for one time instance /c, it is likely to hold for the next time 
instance. Resetting at nearby time instances is not desirable. 
Instead we want to reset once at every step or at some regular 
intervals if the system is stationary for a longer period of time. 
The latter requirement is necessary to distinguish between 
extended stationary periods and extended dynamic periods. 
Further, to allow for real-time processing, the detection needs 
to be done in a recursive manner. The longer the stationary 
period, the smaller the cross-coupling terms in ([6]). This means 
that the system should be reset as late as possible in a 
stationary period. However, if the stationary period is too short, 
we may not want to reset at all, since then the cross terms in ^ 
may not have converged. In summary, necessary conditions for 
a reset are low enough cross-coupling and minimum elapsed 
time since the last reset. If this holds, there is a pending 
reset. In principle, the cross-coupling terms in ^ should be 
used to determine the first requirement. However, in practice, 
all elements fall off together and a threshold 7^ on e.g. the 
first velocity component can be used. To assess the second 
requirement, a counter Cp which is incremented at each time 
instance is needed, giving the pending reset condition 



(P^., < 7p) A {Cp > On 



(9) 



where Cmin is the minimum number of samples between resets. 
A pending reset is to be performed if the stationary period 
comes to an end or a maximum time with a pending reset has 
elapsed. To assess the latter condition, a counter Cd is needed 
which is incremented if ^ holds. Then a reset is performed 
if 

{Z{{i^,6j^}w,) > 7z) V {Cd > C^ax) (10) 

where Cmax is the maximum number of samples of a pending 
reset. Together ^ and ([TO]) make up the sufficient conditions 
for a reset. When the reset is performed, the counters are reset, 
Cp := and Cd := 0. This gives a recursive step segmentation. 
Pseudo-code for the inertial navigation with recursive step 
segmentation (i.e. step- wise inertial navigation) and the step- 
wise dead reckoning is found in Alg. [T] 

Not to lose performance in comparison with a sensor fusion 
approach based on centralized estimation, the step- wise inertial 
navigation combined with the step- wise dead reckoning needs 



Algorithm 1 Pseudo-code of the combined step-wise inertial navigation 
and step- wise dead reckoning. The ZUPT- aided inertial navigation and the 
step-wise dead reckoning refers to the effect of {^-{5} and ^, respectively, 
combined with Kalman type of filtering. For notational compactness, below 

Pfc = P[pfc,vfc,qfc] and Pi = P[x^,x^]. 



Position estimates 



10: 
11 
12: 
13 
14: 
15 
16: 
17 
18 
19: 

20: 
21 

22: 



k := £ := Cp := q := 

Pfc •= v/c := Osxi 

q/e := {Coarse self-initialization} (See e.g. |90| ) 

P/c := {initial velocity, roll and pitch uncertainty} 

{'Ki^Xi) '-— {initial position and heading} 



i^iai) 



{initial position and heading uncertainty} 



'fc-l 



loop 

ZUPT- aided inertial navigation 

([p/e, V/e, q/c], Pfe) ^ ([P/c-1, Vfc-l, q/c-1 
Cp . Cp -\- i. 

if {Py^ < 7p) A (cp > Cmin) then 

Cd '= Cd-l + 1 

if {Z{{i^,6j^}wk) ^ 7z) V (q > Cmax) then 

(ip£ := pfc, #£ := ^/e, P^, = . . . (see ^) 
P/c •= v/c := Osxi 

P/c •= 09x9 



,ffe,^/c) 
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Step-wise dead reckoning 

([x£,X^],P^)^([x£-i,X^- 
end if 
end if 
end loop 
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Fig. 4: The plots show the trajectory (upper), the position error covariances 
(middle), and the covariance between the position and heading errors (lower) 
as estimated by an extended Kalman filter based indefinite ZUPT-aided inertial 
navigation (solid lines) and a step-wise inertial navigation and dead reckoning 
(crossed lines). The agreement between the systems are far below the accuracy 
and integrity of the former system. 



to reproduce the same state statistics (mean and covariance) 
as those of the indefinite (no resets) ZUPT-aided inertial 
navigation. If the models ([T]), ([2]), and ^ had been linear 
with Gaussian noise and the cross-coupling terms of ([6]) were 
perfectly zero, then the divided filtering would reproduce the 
full filter behavior perfectly. Unfortunately, they are not. How- 
ever, as shown in the example trajectory in Fig. |4j in practice 
the differences are marginal and the mean and covariance 
estimates of the position and heading can be reproduced by 
only [dpi^dipi] and the corresponding covariances. 

C Physical decentralization of state estimation 

The step-wise inertial navigation and dead reckoning as 
described in Alg.[T]can be used to implement a decentralized 
architecture and state estimation. The ranging, as well as most 
additional information, is only dependent on position and not 
on the full state vector [p/^, v/c, ^/c]. Further, as argued in the 
previous subsection, the errors of v^ and [(})k-,Ok] are weakly 
correlated with those of pk and ifjk • Therefore, only the states 
[x^,X^] (for all feet) have to be estimated jointly and only 



line 19 need to be executed centrally. The step- wise inertial 
navigation, i.e. Alg.[T] apart from line[T9J can be implemented 
locally in the foot-mounted units, and thereby, only [dp^, di/j^] 
and related covariances need to be transmitted from the feet. 
This way, the required communication will be significantly 
lower compared to the case in which all inertial data would 
have to be transmitted. Also, since the computational cost of 



propagating ^ is marginal, this can be done both locally 
on the processing device of each agent and in a global state 
estimation. This way, if an agent loses contact with whomever 
who performs the global state estimation, it can still perform 
the dead reckoning, and thereby, keep an estimate of where 
it is. Since the amount of data in the displacement and 
heading changes is small, if contact is reestablished, all data 
can easily be transferred and its states in the global state 
estimation updated. The other way around, if corrections to the 
estimates of [x£,X£] are made in the central state estimation, 
these corrections can be transferred down to the agent. Since 
the recursion in ^ is pure dead reckoning (no statistical 
conditioning), these corrections can directly be used to correct 
the local estimates of [x£,X£]. This way, the local and the 
global estimates can be kept consistent. 

The straight-forward way of implementing the global state 
estimation is by one (or multiple) central fusion center to 
which all dead reckoning data are transmitted. The fusion 
center may be carried by an agents, or reside in a vehicle 
or something similar. Range measurements relative to other 
agents only have a meaning if the position estimate and 
its statistics are known. Therefore, all ranging information 
is transferred to the central fusion center. This processing 
architecture with its three layers of estimation (foot, processing 
device of agent, and common fusion center) is illustrated in 
Fig. [5] However, the division in step-wise inertial navigation 
and dead reckoning is independent of the structure with a 
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Fig. 5: Illustration of the decentralized system architecture. Step-wise inertial 
navigation is done locally in the foot-mounted units. Displacement and 
heading changes are transferred to a local processing device, where step- 
wise dead reckoning is performed, and relayed together with ranging data to 
a central fusion center The fusion center may be carried by an agents, reside 
in a vehicle or something similar, or be distributed among agents. 



lower reduction may be expected in practice giving a reduction 
of again around two orders of magnitude. 

IV. Robust and low-cost sensor fusion 

The step-wise dead reckoning provides a low dimensional 
and low update rate interface to the foot-mounted inertial nav- 
igation. With this interface, the global state of the localization 
system (the system state as conceived by the global state 
estimation) becomes 

where Xj and Xj are the positions and headings of the agents' 
feet with dropped time indices. Other auxiliary states may 
also be put in the state vector. Our desire is to fuse the 
provided dead-reckoning with that of the other foot and that of 
other agents via inter-agent ranging. This fusion is primarily 
challenging because: 1) The high dimensionality of the global 
system. 2) The non-collocated sensors of individual agents. 
3) The potentially malign error characteristic of the ranging. 
The high dimensionality is tackled by only propagating mean 
and covariance estimates and by marginalization of the state 
space. The lack of collocation is handled by imposing range 
constraints between sensors. Finally, the error characteristic 
of the ranging is handled by sampling based updates. In the 
following subsections, these approaches are described. Pseudo- 
code for the sensor fusion is found in Algs. [2][3] in the final 
subsection. 



fusion center and some decentralized global state estimation 
could potentially be used. 



D. Computational cost and required communication 

The step-wise dead reckoning is primarily motivated and 
justified by the reduction in computational cost and required 
communication bandwidth. With a completely centralized sen- 
sor fusion [f/c,a;/e], 6 measurement values in total, need to 
be transferred to the central fusion center at a sampling rate 
/iMU in the order of hundreds of [Hz] with each measurement 
value typically consisting of some 12-16 bits. With the step- 
wise dead reckoning, [dp^,^?/^^], Pp^, Pp,,^^, and P^^,^^, in 
total 14 values, need to be transferred to the central fusion 
center at a rate of /sw ^ 1 [Hz] (normal gait frequency |91 ]). 
In practice, the 14 values can be reduced to 8 values since 
cross-covariances may be ignored and the numerical ranges 
are such that they can reasonably be fitted in some 12-16 bits 
each. The other way around, some 4 correction values need to 
be transferred back to the agent. Together this gives a reduction 
of the required communication of (6 • /imu)/(12 • /sw) ^ 10^, 
a two orders magnitude reduction. 

In turn, the computational cost scales linearly with the 
update rates /imu and /sw In addition, the computational 
cost has a cubic scaling (for covariance based filters) with 
the state dimensionality. Therefore, the reduction in the com- 
putational cost at the central fusion center is at the most 
/iMu//sw • (^A) ^ 10^. However, at higher update rates, 
updates may be bundle together. Consequently, a somewhat 



A. Marginalization 

New information (e.g. range measurements) introduced in 
the systems is only dependent on a small subset of the 
states. Assume that the state vector can be decomposed as 
z = [zi, Z2], such that some introduced information tt is only 
dependent on zi. Then with a Gaussian prior with mean and 
covariance 



and Pz 



Z1Z2 



^ Z1Z2 
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(11) 



the conditional (with respect to tt) mean of Z2 and the 
conditional covariance can be expressed as | ,66J 
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where U = P.,.,P-\ V = Z2 - Uzi, Z = Uzi|,V"^ + 
Vz^^U^, and C>zx\'k is the conditional second order moment 
of zi. Note that this will hold for any information tt only 
dependent on zi, not just range constraints as studied in | [66| . 
Consequently, the relations ^V2\ give a desired marginalization. 
To impose the information tt on ([TT]), only the first and second 
conditional moments, 7^i\^ and C^^i^^, need to be calculated. 
If TT is linearly dependent on zi and with Gaussian errors, this 
will be equivalent with a Kalman filter measurement update. 
This may trivially be used to introduce information about 
individual agents. However, as we will show in the following 



subsections, this can also be used to couple multiple navigation 
points of individual agents without any further measurement 
and to introduce ranging information between agents. 

B. Fusing dead reckoning from dual foot-mounted units 

The position of the feet x^ and X5 of an agent (in general 
two navigation points of an agent) has a bounded spatial 
separation. This can be used to fuse the related dead reckoning 
without any further measurements. In practice, the constraint 
will often have different extents in the vertical and the hori- 
zontal direction. This can be expressed as a range constraint 



||D^(Xa-X5)|| <^xy 



(13) 



where D^ is a diagonal scaling matrix with 7 = [1,1, '^xy/lz] 
on the diagonal, and ^xy and 7^ are the constraints in 
the horizontal and vertical direction. Unfortunately, there 
is no standard way of imposing such a constraint in a 
Kalman like framework J92J. Also, the position states being 
in arbitrary locations in the global state vector, i.e. x = 
[. . . , Xq, . . . , X5, . . . ], means that the state vector is not on the 
form of z. Further, since the constraint ([13]) has unbounded 
support, the conditional means [xa|^,X5|^] and co variances 
cov ([xa|^,X5|^]) cannot easily be evaluated. Moreover, since 
the states are updated asynchronously (steps occurring at 
different time instances), the state estimates x^ and X5 may 
not refer to the same time instance. The latter problem can be 
handled by adjusting the constraint 7^^^ by the time difference 
of the states. In principle, this means that an upper limit on 
the speed by which an agent moves is imposed. The former 
problems can be solved with the state transformation 



T^x where 



D. 
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D^ D^ 



n (14) 



where Im-6 is the identity matrix of size m — 6, m is the 
dimension of x, © denotes the direct sum of matrices, 11 is a 
permutation matrix fulfilling 

n[. ..,Xa,...,X5,...] = [Xa,X5,...] 



and zi = D^(xa — X5). With the state transformation T 



7, 



the mean and co variance of z become z = T^x and P^ = 

is invertible, the conditional 

zu and P^K = 



T^P^^T^. Inversely, since T^ 

mean and covariance of x become x^ = T~^ 



|7 = -^7 ^|7 ^"^ ^x\-f 

T^^P^I^T" ' . Therefore, if Zi|^ and C^^i^ are evaluated, ([12]) 
gives Z|^ and P^i^ and thereby also X|^ and Vx\^. Fortunately, 
with zi = D^(xa — X5), the constraint ( [T3] ) becomes 

l|zi|| <lxy 

In contrast to ([TSj, this constraint has a bounded support. 
Therefore, as suggested in f66l, the conditional means can 
be approximated by sampling and projecting sigma points. 
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where ^ denotes approximate equality and 
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Fig. 6: Illustration of the separation constraint update for two feet in the 
horizontal plane. The plots show (upper-left) the prior position and covariance 
estimates, (upper-right) the transformed system with the sigma points (blue 
crosses) and the constraint indicated with the dashed circle, (lower-left) the 
projected sigma points and the conditional mean and covariance, and finally 
(lower-right) the conditioned result in the original domain with the prior 
covariances indicated with thinner lines. 



Here s^*^ and w^^^ are sigma points and weights 

r{zi ,i-3M, z = o 

{s(^),^(^)}=hzi+7^V2i^ ,1/2.}, ^G[l,...,3] 
[{zi-7?V2i^_3 ^1/2^}^ ZG[4,...,6] 

where 1^ is the zth column of the Cholesky decomposition 
P^i = LL^ and the scalar r] reflects the portion of the prior 
to which the constraint is applied. See [ ,66J for further details. 
The application of the constraint for the two-dimensional case 
is illustrated in Fig. [6] 

C Inter-agent range measurement updates 

Similar to the range constraint between the feet of an 
individual agent, the geometry of the inter-agent ranging gives 
the constraints 

r - (7a + 75) < ||xa - X5II < r + (7a + 75) (16) 

where r is the (true) range between agents' ranging devices 
and 7a and 75 are the maximum spatial separation of respective 
ranging device and x^ and X5; where in this case x^ and X5 
are the positions of a foot of each agent. The range only being 
dependent on ||xa — x^H means that the state transformation 
z = Tix where 1 = [1,1,1] and zi = x^ — X5, and 
the corresponding mean and covariance transformations as 
explained in the previous subsection, can be used to let us 
exploit the marginalization ^V2\ . 

The inter-agent ranging gives measurements f of the range 
r. As reviewed in Section |II-B[ the malign attributes of f 



which we have to deal with are potentially heavy tailed error 
distributions and non- stationary spatially correlated errors due 
to diffraction, multi-path, or similar. This can be done by 
using the model r = r ^ v ^ v' where v is a heavy tailed 



error component and v' is a uniformly distributed component 
intended to cover the assumed bounded correlated errors in a 
manner similar to that of [69J . Combining the model with ([16]) 
and the state transformation Ti gives the measurement model 



Prior 



Likelihood 
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(17) 



where 7^ is chosen to cover the bounds in ([16]), the asynchrony 
between x^ and X5, and the correlated errors v' . In practice 7^ 
will be a system parameter trading integrity for information. 
To update the global state estimate with the the range 
measurement f, the state zi and covariance estimates P^i must 
be conditioned on f via ( [TT] ). Due to the stochastic term v, we 
cannot use hard constraints as with the feet of a single agent. 
However, by assigning a uniform prior to the constraint in 
( [TT] ), the likelihood function of f given zi becomes 



/(f|zi)=W(-7.,7r)*V(||zi II -f,<T) 



where V( 1 1 zi 1 1 — f , a^) is the distribution of v with mean 1 1 zi 1 1 — 
f and some scale cr^, ^/(—7r, 7r) is a uniform distribution over 
the interval [—7^,7^], and * denotes convolution. Then, with 
the assumed Gaussian prior zi ~ A/'(zi, P;^^, the conditional 
distribution of zi given f, zi , and P^i is 



/(zi|f)(x/(f|zi)A/'(zi,P,J. 



(19) 



Since zi is low-dimensional, the conditional moments Zi|^ and 
Ozi\r can be evaluated by sampling. With the marginalization 
( [T2I ) and the inverse transformation T^^, this will give the 
conditional mean and covariance of x. 

Since the likelihood function ( [TS] ) is typically heavy tailed, 
it cannot easily be described by a set of samples. However, 
since the prior is (assumed) Gaussian, the sampling of it can 
efficiently be implemented with the eigenvalue decomposition. 
With sample points u*^*^ of the standard Gaussian distribution, 
the corresponding sample points of the prior is given by 



sW = zi 



QA 



l/2u(i) 



where P^^ = Q AQ^ is the eigenvalue decomposition of P^^ . 
With the sample points s*^*\ the associated prior weights only 
become dependent on u^*^ (apart from normalization) since 



w^^ ^ e 
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and can therefore, be precalculated. Reweighting with the 
likelihood function, WpJ = ^pr • /(r|s*^*^) and normalizing 
the weights WpJ = WpJ • (Xl^po )~^, with suitable chosen 
u*^*^ the conditional moments can be approximated by 

Zi|.«E"^^^^'^ and C^,^,^J:wI,^s^'\s^'Y- 

i i 

Consequently, as long as the likelihood function can be 
efficiently evaluated, any likelihood function may be used. 
For analytical convenience, we have typically let V(-, •) be 
Cauchy-distributed giving the heavy-tailed likelihood function 

/(f|s«)~atan(^^^;j^)-atan(^dN;;^b7.j. (20) 

The sampling based range update with this likelihood function 
and u*^*^ from a square sampling lattice is illustrated in Fig. [t] 
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Fig. 7: Illustration of the suggested range update in two dimensions. From left 
to right, the plots show: the Gaussian prior given by the mean zi (blue dot) 
and the covariance Pzi (blue ellipse) and with indicated samples (red dots) 
and eigenvectors/- values, the (one dimensional) likelihood function given by 
the range measurement f and used to reweight the samples, and the resulting 
posterior with conditional mean z^j^ and covariance P^^ir calculated from 
the reweighted samples. 
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Fig. 8: Influence function of the ranging update for ||zi || = 10 [m] performed 
with the suggested method (blue solid/dashed lines) and a traditional Kalman 
measurement update (red dashed-dotted/dotted lines) with P^-^ = I [m^] and 
'Pzi =0.31 [m^], respectively. For the suggested update 7^ = 2 [m] and V2 
was Cauchy distributed with (Tr = 0.5 [m] and for the Kalman measurement 
update the measurement error variance was 1 [m^]. 



Potential more elaborate techniques for choosing the sample 
points is found in |93|. 

The presented ranging update gives a robustness to outliers 
in the measurement data. In Fig. [8j the influence functions 
for the sample based update and the traditional Kalman 
measurement update are shown for the ranging likelihood 
function ( |2Q| ) with 7r = 2 [m] and a^ = 0.5 [m] and position 
covariance values of P^^ = I [m^] and P^^ = 0.31 [m^]. By 
comparing the blue solid and the red dashed-dotted lines, it is 
seen that when the position and ranging error covariances are 
of the same size, the suggested ranging update behaves like the 
Kalman update up to around three standard deviations, where it 
gracefully starts to neglect the range measurement. In addition, 
by comparing the blue dashed and the red dotted lines, it is 
seen that for smaller position error covariances, in contrast 
to the Kalman update, the suggested range update neglects 
ranging measurements with small errors (flat spot in the middle 
of the influence function). This has the effect that multiple 
ranging updates will not make the position error covariance 
collapse, which captures the fact that due to correlated errors, 
during standstill, multiple range measurements will contain 
a diminishing amount of information; and during motion the 
range measurements should only "herd" the dead reckoning. 

With slight modifications, the ranging updates can be 
used to incorporate information from many other information 
sources. Ranging to anchor nodes whose positions are not 
kept in x or position updates (from a GNSS receiver or 
similar) may trivially be implemented as range updates (zero 



10 



Algorithm 2 Pseudo-code for imposing the range constraint fT3) , between 
navigation points Xa and x^, on the global state estimate x. 



z := T^x 



P = T P T^ 



and 
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Sample and project sigma points/weights 
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Algorithm 3 Pseudo-code for conditioning the global state estimate x on the 
range measurements flT) between navigation points Xa and x^. 



z := Tix and P^ 

(Q,A):=eig(P,J 



.{i) .= 



zi + QA^/"u 



V2„(0 



TiP.Tj 
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zi|f := Ei^o^sW and C,,,, := Y^^^o^^'K^^'^Y 
Calculate conditional mean and covariance by marginalization 



1^ := ±;j_ Z|^ and P^^i^ := T-^ P^irT-L 



range in the case of the position update) with zi = x^ — x^ 
replaced with zi = x^ — Xc where Xc is the position of the 
anchor node or the position measurement. Fusion of pressure 
measurements may be implemented as range updates in the 
vertical direction, either relative to other agents or relative to 
a reference pressure. 



D. Summary of sensor fusion 

The central sensor fusion, as described in Section |III-C| 
keeps the position and heading of all feet in the global state 
vector X. From all agents, it receives dead reckoning updates, 
[dY>i,d\l)(\, Pp^, Pp,,^^, and P^^,^^, and inter-agent range 
measurements f. The dead reckoning updates are used to 
propagate the corresponding states and covariances according 
to (|7]). At each dead reckoning update, the range constraint is 



imposed on the state space as described in subsection IV-B 



and corrections are sent back to the agent. The inter-agent 
range measurements are used to condition the state space as 



described in in subsection IV-C Pseudo-code for conditioning 
the state mean and covariance estimates on the range constraint 
and range measurements is shown in Algs. [2][3] 

V. Experimental results 

To demonstrate the characteristics of the sensor fusion 
presented in the previous section, in the following subsection 
we first show numerical simulations giving a quantitative 
description of the fusion. Subsequently, to demonstrate the 
practical feasibility of the suggested architecture and sensor fu- 
sion, a real-time localization system implementation is briefly 
presented. 
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Fig. 9: Illustration of the gain of dual foot-mounted sensors and inter- agent 
ranging. The upper plot shows the step-wise dead reckoning of the individual 
feet without any further information. The middle plot shows the step-wise 
dead reckoning with range constraints between the feet of individual agents. 
The lower plot shows the complete cooperative localization with step-wise 
dead reckoning, range constraints, and inter-agent ranging. 



A. Simulations 

The cooperative localization by foot-mounted inertial sen- 
sors and inter-agent ranging is nonlinear and the behavior 
of the system will be highly dependent on the trajectories. 
Therefore, we cannot give an analytical expression for the 
performance. Instead, to demonstrate the system characteris- 
tics, two extreme scenarios are simulated. For both scenarios 
the agents move with 1 [m] steps at 1 [Hz]. Gaussian errors 
with standard deviation 0.01 [m] and 0.2[°] were added to the 
step displacements and the heading changes, respectively, and 
heavy-tailed Cauchy distributed errors of scale 1 [m] where 
added to the range measurements. The ranging is done time- 
multiplexed in a Round-robin fashion at a total rate of 1 [Hz] . 

1) Straight-line march: N agents are marching beside each 
other in straight lines with agent separation of 10 [m]. The 
straight line is the worst case scenario for the dead reckoning 
and the position errors will be dominated by errors induced 
by the heading errors. In Fig. [9j an example of the estimated 
trajectories of the right (blue) and left (green) feet are shown 
from 3 agents without any further information, with range 
constraints between the feet, and with range constraints and 
inter-agent ranging. The absolute and relative root-mean- 
square-error (RMSE) as a function of the walked distance. 



and for different number of agents, are shown in Fig. 10 
The relative errors are naturally bounded by the inter agent 
ranging. However, the heading RMSE grows linearly with 
time/distance, and therefore, the absolute position error is seen 
to grow with distance. Similar behavior can be observed in 
the experimental data in [ |58| , | [84| . Naturally, the heading 
error, and therefore, also the absolute position RMSE drops 
as 1/\/N where N is the number of agents. This is shown in 



Fig. 1 1 We may also note that the position errors of different 
agents become strongly correlated. The correlation coefficients 
for two agents as a function of distance are shown in Fig. [12 
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Fig. 10: Absolute position RMSE (blue lines) and relative position RMSE (red 
lines) as a function of distance for 100 Monte-Carlo runs. The different blue 
lines correspond, in ascending order, to increasing number of agents. Clearly, 
the relative error is bounded by the inter-agent ranging while the absolute error 
grows slower the larger the number of agents. The final position RMSEs as 
a function of the number of agents are shown in Fig. [TT] 
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Fig. 11: Final position RMSE as a function of the number of agents (blue 
crossed line) for 100 Monte-Carlo runs. From the fit to l/\/]V (dashed red 
line), the position error is seen to be decaying as the square-root of the number 
of agents. 



2) Three static agents: Three non-collinear agents are 
standing still. This will be perceived by the foot-mounted 
inertial navigation, and therefore, they essentially become 
anchor nodes. This is obviously the best-case scenario. A 
fourth agent walk around them in a circle. An example of 



an estimated trajectory is shown in Fig. 13 and the RMSE as 



a function of time is shown in Fig. 14 Since anchor nodes are 
essentially present in the system, the errors are bounded. The 
non-zero RMSE reflects the range constraints in the system. 

From the two scenarios, we can conclude that the relative 
position errors are kept bounded by the inter-agent ranging 
while the absolute position errors (relative starting location) 
are bounded in the best-case (stationary agents); and that the 
error growth is reduced by a factor of I/a/TV in the worst 
case. 

B. Real-time implementation 

The decentralized system architecture has been realized 
with OpenShoe units |TT| and Android smart-phones and 
tablets (Samsung Galaxy S III and Tab 2 10.1) in the in-house 
developed tactical locator system TOR. The communication 
is done over an IEEE 802.11 WLAN. Synthetic inter-agent 
ranging is generated from position measurements from a 
Ubisense system (Ubisense Research & Development Pack- 
age), installed in the KTH Rl reactor hall | [94| . The intension 
is to replace the Ubisense system with in-house developed 
UWB radios |22|. The equipment for a single agent is shown 



in Fig. 15 The multi-agent setup with additional equipment 
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Fig. 12: Position error correlation coefficient for two agents in the straight- 
line march scenario in the x direction (solid blue), y direction (dashed green), 
and z direction (dotted/dashed red). Clearly, the positions errors of different 
agents become strongly correlated with increasing distance traveled. 
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Fig. 13: The plot shows an estimated trajectory from the scenario with three 
static agents and a fourth mobile agent walking around them. Clearly, the 
position estimation errors are bounded. 



The step-wise inertial navigation and the associated transfer 
of displacements and heading changes has been implemented 
in the OpenShoe units. The agent filtering has been imple- 
mented as Android applications together with graphical user 
interfaces. A screen- shot of the graphical user interface with 
trajectories from a 10 [min] walk in the reactor hall and 

Currently, 



adjacent rooms of 4 agents is shown in Fig. 17 



for sensor mounting is shown in Fig. 16 



the central sensor fusion is running on a laptop, but there is 
ongoing work of migrating it to an Android tablet. 

VI. Conclusions 

Key implementation challenges of cooperative localization 
by foot-mounted inertial sensors and inter-agent ranging are: 
designing an overall system architecture to minimize the re- 
quired communication and computational cost, while retaining 
the performance and making it robust to varying connectiv- 
ity; and fusing the information from the system under the 
constraint of the system architecture while retaining a high 
integrity and accuracy. A solution to the former problem has 
been presented in the partially decentralized system architec- 
ture based on the division and physical separation of the step- 
wise inertial navigation and the step- wise dead reckoning. 
A solution to the latter problem has been presented in the 
marginalization and sample based spatial separation constraint 
and ranging updates. By simulations, it has been shown that 
in the worst case scenario, the absolute localization RMSE 
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Fig. 14: The plot shows the RMSE of the mobile unit for the three static 
agents scenario over 100 Monter-Carlo runs. Being static, the three stationary 
agents essentially become anchor nodes, and therefore, the RMSE is bounded. 

USB hub 




OpenShoe units Ubisense radio tag 

(foot-mounted inertial sensors) (synthetic inter-agent ranging) 

Fig. 15: Agent equipment carried by each agent in the prototype implemen- 
tation. The OpenShoe units are connected to the USB -hub. Radio tags and 
a Ubisense real-time location system are used to generate synthetic range 
measurements between agents. 



improves as the square-root of the number of agents and the 
relative errors are bounded. In the best case scenario, both 
the relative and the absolute errors are bounded. Finally, the 
feasibility of the suggested architecture and sensor fusion has 
been demonstrated with simulations and a real-time system 
implementation featuring 4 agents and a meter level accuracy 
for operation times of tenth of minutes in a harsh industrial 
environment. 
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